#include "commdata.h"

#include <cstring>

constexpr uint8_t PLACEHOLDER = 0x00;                   // 占位符
constexpr uint8_t X_UPPER = 0x58;                       // X
constexpr uint8_t X_LOWER = 0x78;                       // x
constexpr size_t PLACEHOLDER_LEN = 1;                   // 开头占位符
constexpr size_t HEADER_LEN = 10;                       // 报文头长度
constexpr size_t CHECKSUM_LEN = 1;                      // 校验和长度
constexpr size_t DATA_LEN_MAX = UINT16_MAX - PLACEHOLDER_LEN - HEADER_LEN - CHECKSUM_LEN;   // 最大数据长度

std::vector<uint8_t> CommData::send(uint8_t cmd_1, uint8_t cmd_2, uint8_t cmd_3, const std::vector<uint8_t> &data)
{
    const size_t data_len = data.size();

    if (data_len > DATA_LEN_MAX) {
        return {};
    }

    const size_t total_len = PLACEHOLDER_LEN + HEADER_LEN + data_len + CHECKSUM_LEN;

    std::vector<uint8_t> bytes(total_len);

    size_t pos = 0;
    // header
    bytes[pos++] = X_UPPER;     // 1 flag_0
    bytes[pos++] = X_LOWER;     // 2 flag_1
    bytes[pos++] = PLACEHOLDER; // 3 number
    bytes[pos++] = PLACEHOLDER; // 4 address
    bytes[pos++] = PLACEHOLDER; // 5 reserve
    bytes[pos++] = cmd_1;       // 6
    bytes[pos++] = cmd_2;       // 7
    bytes[pos++] = cmd_3;       // 8
    bytes[pos++] = static_cast<uint8_t>(data_len & 0xFF);       // 9  length high
    bytes[pos++] = static_cast<uint8_t>(data_len >> 8 & 0xFF);  // 10 length low
    // body
    if (data_len > 0) {
        std::memcpy(&bytes[pos], data.data(), data_len);
    }
    // checksum
    uint8_t checksum = 0;
    for (size_t i = 1; i < bytes.size() - 1; ++i) {
        checksum ^= bytes[i];
    }
    bytes[total_len - 1] = checksum;

    return bytes;
}

std::optional<Message> CommData::recv(const std::vector<uint8_t>& msg)
{
    if (msg.size() < HEADER_LEN + CHECKSUM_LEN) {
        return std::nullopt;
    }

    for (size_t i = 0; i <= msg.size() - 2; ++i) {
        if (msg[i] != X_LOWER || msg[i+1] != X_UPPER) {
            continue;
        }

        MessageHeader header;
        size_t pos = i;
        header.code_0 = msg[pos++];
        header.code_1 = msg[pos++];
        header.number = msg[pos++];
        header.address = msg[pos++];
        header.reserve = msg[pos++];
        header.cmd_1 = msg[pos++];
        header.cmd_2 = msg[pos++];
        header.cmd_3 = msg[pos++];
        // 读取uint16_t（小端模式）
        uint8_t lowByte = msg[pos++];
        uint8_t highByte = msg[pos++];
        header.length = static_cast<uint16_t>(lowByte) | (static_cast<uint16_t>(highByte) << 8);

        // 检查总长度是否足够
        const size_t totalRequiredLen = HEADER_LEN + header.length + CHECKSUM_LEN;
        if (i + totalRequiredLen > msg.size()) {
            continue;
        }

        // 提取数据段（高效构造）
        const size_t dataStart = i + HEADER_LEN;
        const size_t dataEnd = dataStart + header.length;
        std::vector<uint8_t> data(msg.begin() + dataStart, msg.begin() + dataEnd);

        // 验证校验和
        const uint8_t readChecksum = msg[dataEnd];
        if (readChecksum != checksum(header, data)) {
            continue;
        }

        // 解析成功
        return Message{header, data, readChecksum, i, dataEnd + 1};
    }

    return std::nullopt;
}

uint8_t CommData::checksum(const MessageHeader& header, const std::vector<uint8_t>& data) {
    uint8_t sum = 0;
    sum ^= header.code_0;
    sum ^= header.code_1;
    sum ^= header.number;
    sum ^= header.address;
    sum ^= header.reserve;
    sum ^= header.cmd_1;
    sum ^= header.cmd_2;
    sum ^= header.cmd_3;
    sum ^= (header.length >> 8) & 0xFF; // 高字节
    sum ^= header.length & 0xFF;        // 低字节
    for (uint8_t byte : data) {
        sum ^= byte;
    }
    return sum;
}

uint8_t CommData::checksum(const std::vector<uint8_t> &data)
{
    uint8_t sum = 0;
    for (uint8_t byte : data) {
        sum ^= byte;
    }
    return sum;
}
